
#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <NewTone.h>

#include "mBot.h"
#include "MePort.h"
#include "MeRGBLed.h"
#include "MeUltrasonic.h"
#include "MeBuzzer.h"

#include "protothreads.h"


MeBoard myBoard(mBot);
MeRGBLed rgbled_7(7, 7 == 7 ? 2 : 4);
MeUltrasonic ultrasonic_3(3);
LiquidCrystal_I2C mylcd(0x27, 16, 2);
MeBuzzer buzzer;
MePort linefollower_1(2); // port2

pt pt_led, pt_meadis, pt_display, pt_ambulance;


int disp_loop_num = 0;
int disp_distance = 0;


void setup() {
  mylcd.init();
  mylcd.backlight();
  pinMode(8, OUTPUT);

  PT_INIT(&pt_led);
  PT_INIT(&pt_meadis);
  PT_INIT(&pt_display);
  PT_INIT(&pt_ambulance);
}


int thread_led(struct pt *pt, int interval) {
  PT_BEGIN(pt);
  int led_bright = 30;

  for (;;) {
    disp_loop_num ++;

    // section1
    rgbled_7.setColor(0, led_bright, 0, 0);
    rgbled_7.show();
    PT_SLEEP(pt, 100);

    // section2
    rgbled_7.setColor(0, 0, led_bright, 0);
    rgbled_7.show();
    PT_SLEEP(pt, 100);

    // section3
    rgbled_7.setColor(0, 0, 0, led_bright);
    rgbled_7.show();
    PT_SLEEP(pt, 100);
  }
  PT_END(pt);
}

int thread_ambulance(struct pt *pt) {
  PT_BEGIN(pt);

  for (;;) {
    disp_loop_num ++;

    // section1
    buzzer.tone(950);
    PT_SLEEP(pt, 600);

    buzzer.tone(700);
    PT_SLEEP(pt, 400);
    
    //NewTone(8, 950, 600);
    //NewTone(8, 700, 400);
    //PT_SLEEP(pt, 100);
  }
  PT_END(pt);
}

int thread_meadis(struct pt *pt, int interval) {
  static unsigned long timestamp = 0;
  PT_BEGIN(pt);
  while (1) {
    PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
    timestamp = millis();

    disp_distance = ultrasonic_3.distanceCm();
  }
  PT_END(pt);
}

int thread_display(struct pt *pt, int interval) {
  static unsigned long timestamp = 0;
  char buf[32];

  PT_BEGIN(pt);
  while (1) {
    PT_WAIT_UNTIL(pt, millis() - timestamp > interval );
    timestamp = millis();

    int ld = linefollower_1.dRead1()*2+linefollower_1.dRead2(); // 0 - all black, 1 - left black, 2 - right black, 3 - all black

    mylcd.setCursor(0, 0);
    sprintf(buf, "ld: %05d", ld);
    mylcd.print(buf);

    mylcd.setCursor(0, 1);
    sprintf(buf, "dist: %03d cm", disp_distance);
    mylcd.print(buf);
  }
  PT_END(pt);
}


void loop() {
  PT_SCHEDULE(thread_led(&pt_led, 100));
  PT_SCHEDULE(thread_meadis(&pt_meadis, 100));
  PT_SCHEDULE(thread_display(&pt_display, 200));
  PT_SCHEDULE(thread_ambulance(&pt_ambulance));
}
